#pragma once

#include "./track.cpp"
#include "./imgprocess.cpp"
#include "./control.cpp"
#include "./motion.cpp"
#include "./circle.cpp"
#include "../include/uart.hpp"
#include "../include/common.hpp"     //公共类方法文件
#include <opencv2/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc.hpp>
#include <unistd.h>

#include "../include/detection.hpp"  //百度Paddle框架移动端部署
#include "./rescue.cpp"
#include "./danger.cpp"
#include "./zebracrossing.cpp"
#include "./bridge.cpp"
#include "./racing.cpp"

#include <thread>
#include <mutex>
#include "./img_thread.cpp"

using namespace std;
using namespace cv;


/**
 * detection返回的标定框xy是左上角
 * 
 */


int main()
{
    VideoCapture capture;       // Opencv相机类
    imgprocess imgprocess;      //使用传参的形式传递数组，不用全局变量
    track track;             //边线需要传递，使用别名传递 因为在类里面写的函数不认他
    control control;
    Motion motion;
    Circle circle;
    Scene scene=NormalScene;
    
    Rescue rescue;
    Danger danger;
    Parking park;
    Bridge bridge;
    Racing racing;

    mutex mtx_img;//线程锁

//摄像头
    if(motion.params.saveImg){
        capture = VideoCapture(motion.params.video); // 打开本地视频
    }else{
        capture = VideoCapture("/dev/video0"); // 打开摄像头
    }
    if (!capture.isOpened()) 
    {
        printf("can not open video device!!!\n");
        return 0;
    }
    capture.set(CAP_PROP_FRAME_WIDTH, 640);  // 设置图像分辨率
    capture.set(CAP_PROP_FRAME_HEIGHT, 480); // 设置图像分辨率
    capture.set(CV_CAP_PROP_FOURCC, CV_FOURCC('M', 'J', 'P', 'G'));
    capture.set(CV_CAP_PROP_FPS, 120);//帧率 帧/秒
    //摄像头硬件缺陷导致320*240读取只有50帧，640*320的120帧正常
    Mat frame;

//上下机位通信
    shared_ptr<Uart> uart = make_shared<Uart>("/dev/ttyUSB0"); // 初始化串口驱动
    int ret = uart->open();
    if (ret != 0) {
        printf("[Error] Uart Open failed!\n");
        return -1;
    }
    uart->startReceive(); // 启动数据接收子线程
    //智能指针里uart；uart里面套智能指针；智能指针里面套thread;
    

    // 目标检测类(AI模型文件)
    shared_ptr<Detection> detection = make_shared<Detection>(motion.params.model);
    detection->score = motion.params.score; // AI检测置信度
    Mat detection_img; //如果将track的img传过去会导致段错误（不知道为什么）
    //1.双摄像头，双摄像头同时申请的带宽太大总线带宽会被占满
    //2.ai的多线程 现在的双线程只是将摄像头和ai分开
    img_thread detection_thread;
    detection_thread.init(motion);
    
    long preTime;
    float speed;
    while(1)
    {
        uart->carControl(0, PWMSERVOMID); // 通信控制车辆停止运动
        while (!uart->keypress)
        {
            waitKey(300);
        }
        uart->keypress=false;
        uart->buzzerSound(uart->BUZZER_START); // 祖传提示音效
        waitKey(1000);
        if(uart->keypress)
        {
            uart->carControl(0, PWMSERVOMID); // 通信控制车辆停止运动
            uart->buzzerSound(uart->BUZZER_FINISH); // 祖传提示音效
            break;
        }
        uart->buzzerSound(uart->BUZZER_OK); // 祖传提示音效
        scene=NormalScene;//场景置0
        for(int jj=0;jj<NUM;jj++)
        {
            capture.read(frame);
            resize(frame, detection_img, Size(COLSIMAGE,ROWSIMAGE), 0, 0, INTER_NEAREST);
            detection_thread.img[jj]=detection_img.clone();
            detection_thread.thread_creat(jj);
        }
        danger.init();
        racing.init();
        
    while(1)
    {
        speed=motion.params.speedHigh;//速度
// auto startTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//帧率计算
// printf(">>main FrameTime: %ldms | %.2ffps \n", startTime - preTime,1000.0 / (startTime - preTime));
// preTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//程序开始时间

//20ms
        if(!capture.read(frame))//获取图片
        {continue;}
//8ms
        resize(frame, track.img, Size(COLSIMAGE,ROWSIMAGE), 0, 0, INTER_NEAREST);
        detection_img = track.img.clone();//复制一张img
        if(motion.params.cross)
        {   
                detection->results=detection_thread.thread_ask(detection_img);//询问ai线程
        }
        track.binary_mat = imgprocess.binaryzation(track.img); // 图像二值化
        imgprocess.mat_to_arry(track);//mat和arry都在track里面
        track.basemap_generate(motion.params.rowCutBottom);//转化为basemap
        // track.find_mid_line();//找边线岔点
        // track.Find_Mid_Line();

        switch(scene)
        {
        case NormalScene://上一帧无场景，判断这一帧场景
            if(rescue.if_rescue(detection->results)!=0&&motion.params.rescue==true&&motion.params.cross==true)
            {
                scene=RescueScene;
                uart->buzzerSound(uart->BUZZER_START); // 祖传提示音效
                break;
            }else if(danger.if_danger(detection->results)!=0&&motion.params.danger==true&&motion.params.cross==true)
            {
                scene=DangerScene;
                uart->buzzerSound(uart->BUZZER_START); // 祖传提示音效
                break;
            }else if(park.process(detection->results)!=0&&motion.params.parking==true)
            {
                scene=ParkingScene;
                uart->buzzerSound(uart->BUZZER_START); // 祖传提示音效
            }else if(racing.if_racing(track,detection->results)!=0&&motion.params.racing==true)
            {
                scene=RacingScene;
                uart->buzzerSound(uart->BUZZER_START); // 祖传提示音效
            }else if(circle.RoadIsRoundabout(track)!=0&&motion.params.ring==true)//圆环
            {
                scene=RingScene;
                uart->buzzerSound(uart->BUZZER_START); // 祖传提示音效
                // uart->carControl(0, PWMSERVOMID); // 通信控制车辆停止运动
                // uart->keypress=true;
                break;
            }else if(bridge.process(detection->results)!=0&&motion.params.bridge==true)
            {
                scene=BridgeScene;
            }
        break;
        case RingScene://上一帧是圆环，对圆环处理，
            speed=motion.params.speedLow;
            circle.RoundaboutProcess(track);
            if(circle.state==0)//如果从圆环中出来了回到无场景
            {scene=Scene::NormalScene;}
        break;
        case RescueScene:
            speed=motion.params.speedBridge;
            rescue.process(track,detection->results,false);
            if(rescue.step==0){
                rescue.rescue_init();
                scene=Scene::NormalScene;
            }else if(rescue.step==5){
                speed=0;
            }else if(rescue.step==6){
                speed=(-1.0*motion.params.speedBridge);
            }
            //停不准的话加上偏差值的判断
        break;
        case DangerScene:
            speed=motion.params.speedDown;
            //判断result是否正在被写入
            if(danger.process(track,detection->results)==1)
            {scene=Scene::NormalScene;}
        break;
        case ParkingScene:
            uart->keypress=true;
        break;
        case BridgeScene:
            speed=motion.params.speedBridge;
        break;
        case RacingScene:
            speed=0.5;
            if(racing.process(track,detection->results)==true)
            {
                scene=Scene::NormalScene;
                uart->buzzerSound(uart->BUZZER_DING); 
            }
            if(racing.step==5){speed=0;}
        break;
        }
        control.get_devation(track);//算偏差
        motion.poseCtrl(control.devation);//偏差算舵机pwm
        if(motion.params.debug){
            if(!motion.params.saveImg)
            {
                uart->carControl(0, motion.servoPwm);//debug==true的时候车不动        
            }
            imgprocess.display_window(racing,park,danger,rescue,circle,motion,control, track ,"find lines stest",scene);//展示单张图像
            
            cv::Mat mat(ROWSIMAGE, COLSIMAGE, CV_8UC1, (unsigned char *)track.basemap );
            imshow("trytry",mat);
if(motion.params.cross)
{
            detection->drawBox(detection_img); // 图像绘制AI结果
            imshow("ai_detection", detection_img);
            waitKey(10);    // 等待显示
}
        }else{
            uart->carControl_thread(speed, motion.servoPwm);//传到下位机
        }
        if(uart->keypress)
        {
            uart->keypress=false;
            waitKey(100);
            uart->carControl(0, PWMSERVOMID); // 通信控制车辆停止运动
            break;
        }

    }
    park.park_init();
    danger.init();
    bridge.bridge_init();
    rescue.rescue_init();
    detection->results_clear();
    detection_thread.close();
    }
    capture.release();
    // uart->close(); // 串口通信关闭
    return 0;
}

